#include "my_utils.h"
#include <algorithm>

// ===========================================
//                  S型速度规划
// ===========================================

SpeedUtils::SpeedUtils(int speed_max, int acc_max, int jerk)
{
    this->speed_max_ = speed_max;
    this->speed_min_ = 50;
    this->acc_max_ = acc_max;
    this->jerk_ = jerk;
    this->cur_speed = 0;
    this->cur_acc = 0;
    this->delta = 0.1;

    double t1 = acc_max / jerk;
    double t2 = (speed_max - t1 * acc_max) / acc_max;
    this->choice[0] = jerk * pow(t1, 3);
    this->choice[1] = 0.5 * acc_max * pow(t1, 2) + 0.5 * acc_max * pow(t2, 2);
}

SpeedUtils::~SpeedUtils()
{
}

/**
 * @brief  len[start, end]数组相加
 * @param  start: 
 * @param  end: 
 * @return double: 
 */
double SpeedUtils::len_sum(int start, int end)
{
    double sum = 0;
    for (int i = start; i <= end; i++)
    {
        sum = sum + this->len[i];
    }
    return sum;
}

void SpeedUtils::setGoal(double overall)
{
    this->overall = overall;
    if (overall < this->choice[0])
    {
        // 未到达规定的amax
        this->time[1] = pow((overall / this->jerk_), 1.0 / 3);
        this->time[2] = 0;
        this->time[3] = this->time[1];
        this->time[4] = 0;
        this->time[5] = this->time[1];
        this->time[6] = 0;
        this->time[7] = this->time[1];

        this->cur_acc_max = this->jerk_ * this->time[1];
        this->len[1] = overall / 6;
        this->len[2] = 0;
        this->len[3] = this->cur_acc_max * pow(this->time[3], 2) - this->jerk_ * pow(this->time[3], 3) / 6;
        this->len[4] = 0;
        this->len[5] = this->len[3];
        this->len[6] = 0;
        this->len[7] = this->len[1];
    }
    else if (overall < this->choice[1])
    {
        // 到达规定的amax,未到达规定的vmax
        this->cur_acc_max = this->acc_max_;
        this->time[1] = (double)this->cur_acc_max / this->jerk_;
        this->time[2] = sqrt((overall / this->cur_acc_max) + pow(this->time[1], 2) / 4) - 3 * this->time[1] / 2;
        this->time[3] = this->time[1];
        this->time[4] = 0;
        this->time[5] = this->time[1];
        this->time[6] = this->time[2];
        this->time[7] = this->time[3];

        this->len[1] = this->cur_acc_max * pow(this->time[1], 2) / 6;
        this->len[2] = this->cur_acc_max * (this->time[1] * this->time[2] + pow(this->time[2], 2)) / 2;
        this->len[3] = this->cur_acc_max * this->time[3] * (this->time[1] + this->time[2]) - this->jerk_ * pow(this->time[3], 3) / 6;
        this->len[4] = 0;
        this->len[5] = this->len[3];
        this->len[6] = this->len[2];
        this->len[7] = this->len[1];
    }
    else
    {
        // 到达规定的vmax
        this->cur_acc_max = this->acc_max_;
        this->time[1] = (double)this->cur_acc_max / this->jerk_;
        this->time[2] = (this->speed_max_ - this->cur_acc_max * this->time[1]) / this->cur_acc_max;
        this->time[3] = this->time[1];
        this->time[4] = (overall - this->cur_acc_max * (3 * this->time[1] * this->time[2] + pow(this->time[2], 2) + 2 * pow(this->time[1], 2))) / this->speed_max_;
        this->time[5] = this->time[1];
        this->time[6] = this->time[2];
        this->time[7] = this->time[3];

        this->len[1] = this->cur_acc_max * pow(this->time[1], 2) / 6;
        this->len[2] = this->cur_acc_max * (this->time[1] * this->time[2] + pow(this->time[2], 2)) / 2;
        this->len[3] = this->cur_acc_max * this->time[3] * (this->time[1] + this->time[2]) - this->jerk_ * pow(this->time[3], 3) / 6;
        this->len[4] = this->speed_max_ * this->time[4];
        this->len[5] = this->len[3];
        this->len[6] = this->len[2];
        this->len[7] = this->len[1];
    }
    
}

int SpeedUtils::getSpeed(int walked)
{
    if (walked < this->len[1])
    {
        this->cur_speed = this->cur_speed + (int)this->cur_acc * this->delta;
        this->cur_acc = this->jerk_ * this->delta + this->cur_acc;
    }
    else if (walked < this->len_sum(1, 2))
    {
        this->cur_speed = this->cur_speed + (int)this->cur_acc * this->delta;
        this->cur_acc = this->cur_acc_max;
    }
    else if (walked < this->len_sum(1, 3))
    {
        this->cur_speed = this->cur_speed + (int)this->cur_acc * this->delta;
        this->cur_speed = std::min(this->cur_speed, this->speed_max_);
        this->cur_acc = this->cur_acc - this->jerk_ * this->delta;
    }
    else if (walked < this->len_sum(1, 4))
    {
        this->cur_speed = this->speed_max_;
        this->cur_acc = 0;
    }
    else if (walked < this->len_sum(1, 5))
    {
        this->cur_speed = this->cur_speed + (int)this->cur_acc * this->delta;
        this->cur_acc = this->cur_acc - this->jerk_ * this->delta;
    }
    else if (walked < this->len_sum(1, 6))
    {
        this->cur_speed = this->cur_speed + (int)this->cur_acc * this->delta;
        this->cur_acc = - this->cur_acc_max;
    }
    else
    {
        this->cur_speed = this->cur_speed + (int)this->cur_acc * this->delta;
        this->cur_speed = std::max(this->cur_speed, this->speed_min_);
        this->cur_acc = this->cur_acc + this->jerk_ * this->delta;
    }
    return this->cur_speed;
}


std::vector<unsigned char> string2Bytes(std::string const &s)
{
    std::vector<unsigned char> bytes;
    for (char c : s)
    {
        bytes.push_back(static_cast<unsigned char>(c));
    }
    return bytes;
}

std::string byte2String(unsigned char *recv, int start, int end)
{
    return std::string(reinterpret_cast<const char*>(recv + start), end - start);
}